import threading
import queue
import time
from time import sleep
from gpiozero import DigitalOutputDevice
import sys

# 引脚定义
DIR = 20
STEP = 21
CW = True   # 顺时针方向
CCW = False # 逆时针方向
SPR = 1600  # 每转步数

class StepperMotor:
    def __init__(self, dir_pin=DIR, step_pin=STEP, spr=SPR):
        """
        初始化步进电机控制器
        :param dir_pin: 方向控制引脚 (BCM编号)
        :param step_pin: 步进脉冲引脚 (BCM编号)
        :param spr: 每转步数
        """
        print("初始化步进电机对象...")
        self.dir_pin = DigitalOutputDevice(dir_pin)
        self.step_pin = DigitalOutputDevice(step_pin)
        self.spr = spr
        self.current_position = 0  # 当前位置（步数）
        # 初始状态设置
        self.set_direction(CW)
        print(f"步进电机已初始化 | DIR:BCM{dir_pin}, STEP:BCM{step_pin}, SPR:{spr}")
    
    def set_direction(self, direction):
        """设置电机转动方向"""
        self.dir_pin.value = direction
        self.current_direction = direction
        direction_name = "顺时针(CW)" if direction == CW else "逆时针(CCW)"
        # print(f"方向设置为: {direction_name}")
        return self
    
    def step(self, delay=0.001):
        """发送一个步进脉冲"""
        self.step_pin.on()
        sleep(delay)
        self.step_pin.off()
        sleep(delay)
        
        # 更新位置
        direction_factor = 1 if self.current_direction == CW else -1
        self.current_position += direction_factor
    
    def move_steps(self, steps, delay=0.001, stop_event=None):
        """
        移动指定步数
        :param steps: 移动步数 (正数)
        :param delay: 步进延迟 (秒)，控制速度
        """
        if steps <= 0:
            print("错误: 步数必须大于0")
            return
            
        print(f"移动 {steps} 步 | 每步延迟: {delay*1000:.3f}ms")
        
        for i in range(steps):
            # 检查中断信号
            if stop_event and stop_event.is_set():
                print("电机运行被中断")
                return False

            self.step(delay)
            # 显示进度
            if i % 100 == 0 or i == steps - 1:
                progress = (i + 1) / steps * 100
                sys.stdout.write(f"\r进度: {progress:.1f}% [{'='*int(progress/2)}{' '*(50-int(progress/2))}]")
                sys.stdout.flush()
        
        print()  # 换行
        return self
    
    def move_degrees(self, degrees, delay=0.001, stop_event=None):
        """
        旋转指定角度
        :param degrees: 旋转角度
        :param delay: 步进延迟 (秒)
        支持外部中断的步进电机控制
        :param stop_event: 可选,threading.Event对象,用于外部中断
        """
        steps = int((abs(degrees) / 360.0) * self.spr)
        direction = CW if degrees >= 0 else CCW
        
        print(f"旋转 {degrees}° ≈ {steps} 步")
        self.set_direction(direction).move_steps(steps, delay, stop_event)
        return self

    def release(self):
        """释放资源"""
        self.dir_pin.close()
        self.step_pin.close()
        print("资源已释放")

# ==================== 测试代码 ==================== 
# if __name__ == "__main__":
#     # 创建步进电机对象
#     motor = StepperMotor(dir_pin=DIR, step_pin=STEP, spr=SPR)
    
#     try:
#         # 测试1：顺时针旋转180度（中等速度）
#         print("\n=== 测试1: 顺时针旋转180度 ===")
#         motor.move_degrees(180, delay=0.001)
        
#         # # 测试3：精确移动3200步（慢速）
#         # print("\n=== 测试3: 精确移动3200步 ===")
#         # motor.move_steps(3200, delay=0.002)
        
#         # # 测试4：加速旋转
#         # print("\n=== 测试4: 加速旋转 ===")
#         # motor.set_direction(CW)
#         # motor.move_steps(SPR, 0.001)     # 初始速度
#         # motor.move_steps(SPR*2, 0.0005)  # 加速
#         # motor.move_steps(SPR*3, 0.0001)  # 高速
        
#         # 显示最终位置
#         print(f"\n最终位置: {motor.get_position()} 步")
    
#     except KeyboardInterrupt:
#         print("\n程序被用户中断")
    
#     finally:
#         motor.release()

class MotorController:
    def __init__(self, stop_event, status_queue):
        """
        初始化电机控制器。
        :param stop_event: 用于通知线程停止的 Event 对象。
        :param status_queue: 用于传递共享状态的队列。与传感器共享
        """
        self.stop_event = stop_event
        self.status_queue = status_queue
        self.parameters_queue = queue.Queue()  # 用于接收参数的队列
        self.pause_queue = queue.Queue()       # 用于接收停止消息的队列
        self.stepper = StepperMotor(dir_pin=DIR, step_pin=STEP, spr=SPR)  # 初始化步进电机控制器
        self.running = False  # 标记电机是否正在运行
        self.worker_thread = None  # 工作线程
        self.is_low_limit = False # 标记是否到达下限位
        self.is_upper_limit = False # 标记是否到达上限位
        
        # 启动工作线程
        self.start_worker()

    def start_worker(self):
        """启动工作线程"""
        if self.worker_thread is None or not self.worker_thread.is_alive():
            self.worker_thread = threading.Thread(target=self._worker, daemon=True)
            self.worker_thread.start()
            print("电机工作线程已启动")

    def _worker(self):
        """工作线程的主逻辑"""
        while not self.stop_event.is_set():
                # 阻塞等待指令，避免空循环
                steps, delay, direction = self.parameters_queue.get()
                print(f"处理参数: {steps}步/{delay*1000:.3f}ms/{direction}")
                
                # 设置方向
                self.stepper.set_direction(direction)
                
                # 控制电机运行
                self.running = True
                for i in range(steps):
                    if not self.pause_queue.empty():
                        message = self.pause_queue.get_nowait()
                        if message == "PAUSE":
                            print("接收到暂停消息，跳出当前运动循环")
                            # 清空暂停队列
                            while not self.pause_queue.empty():
                                try:
                                    self.pause_queue.get_nowait()
                                except queue.Empty:
                                    break
                            break

                    if not self.status_queue.empty():
                         #从队列获取消息
                        source, command  = self.status_queue.get_nowait()
                        if command  == "UPPER" and direction == False: #到达上限位且电机后退运动 todo
                            print("达到上限位，暂停当前运动循环")
                            self.is_upper_limit = True
                            # 清空暂停队列
                            while not self.status_queue.empty():
                                try:
                                    self.status_queue.get_nowait()
                                except queue.Empty:
                                    break
                            break
                        
                        if command  == "LOWER" and direction == True:#到达上限位且电机前进运动 todo
                            print("达到前进限位，暂停当前运动循环")
                            self.is_low_limit = True
                            # 清空暂停队列
                            while not self.status_queue.empty():
                                try:
                                    self.status_queue.get_nowait()
                                except queue.Empty:
                                    break
                            break

                    # 检查中断信号或停止消息
                    if self.stop_event.is_set():
                        print("电机运行被中断")
                        self.running = False
                        return
                    self.is_upper_limit = False
                    self.is_low_limit = False
                    # 发送脉冲
                    self.stepper.step(delay)
                    self.running = True #电机正在运行
                    #实施打印状态
                    # print(f"发送脉冲 {i + 1}/{steps} | 延迟: {delay * 1000:.3f}ms | 方向: {direction}")
#############################################
                # 运行结束，重置参数
                steps = 0
                self.running = False
                # print("电机运行完成")

            # except queue.Empty:
            #     # 队列为空时，等待一段时间
            #     time.sleep(0.1)
            # except Exception as e:
            #     print(f"电机运行异常：{e}")
            #     self.running = False

    def set_parameters(self, steps, delay, direction):
        """设置电机参数（通过队列传递）"""
        self.parameters_queue.put((steps, delay, direction))
        print(f"设置电机参数: steps={steps}, delay={delay}, direction={direction}")

    def pause(self):
        """通过队列发送暂停消息，跳出电机运行循环"""
        self.pause_queue.put("PAUSE")
        print("已通过队列发送暂停消息")

    def release(self):
        """释放资源"""
        self.stop()
        if self.worker_thread and self.worker_thread.is_alive():
            self.worker_thread.join(timeout=1)
        self.stepper.release()
        print("电机资源已释放")


    def stop(self):
        """停止电机"""
        self.stop_event.set()
        print("电机停止信号已发送")